/*******************************************************************/
/*                       Author: gz                                */
/*                 Contact: gz_robot@outlook.com                   */
/*                   First version: 2019-04-16                     */
/*                    Last update: 2019-05-21                      */
/*******************************************************************/

#ifndef CLASS_OBJ_VISUALIZATION_H
#define CLASS_OBJ_VISUALIZATION_H

#include <vector>
#include <string>
#include <sstream>
#include <cmath>
#include <iomanip>

#include <ros/ros.h>
// #include <ros/param.h>
// #include <ros/service.h>
// #include <ros/service_server.h>

#include <tf/transform_datatypes.h>

#include <std_msgs/Header.h>

#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>

#include "msg_lidar_shape/DetectedObject.h"
#include "msg_lidar_shape/DetectedObjectArray.h"

#define __APP_NAME__ "visualize_detected_objects"

class ObjVisualization
{
private:
public:
  const double arrow_height_;
  const double label_height_;
  const double object_max_linear_size_ = 50.;
  double       object_speed_threshold_;
  double       arrow_speed_threshold_;
  double       marker_display_duration_;

  int marker_id_;

  std_msgs::ColorRGBA label_color_, box_color_, hull_color_, arrow_color_, centroid_color_, model_color_;

  std::string input_topic_, ros_namespace_;
  std::string markers_out_topic;

  ros::NodeHandle node_handle_;
  ros::Subscriber subscriber_detected_objects_;

  ros::Publisher publisher_markers_;
  void           SetColor(std::vector<double> &color);

  visualization_msgs::MarkerArray ObjectsToLabels(const msg_lidar_shape::DetectedObjectArray &in_objects);

  visualization_msgs::MarkerArray ObjectsToArrows(const msg_lidar_shape::DetectedObjectArray &in_objects);

  visualization_msgs::MarkerArray ObjectsToBoxes(const msg_lidar_shape::DetectedObjectArray &in_objects);

  visualization_msgs::MarkerArray ObjectsToModels(const msg_lidar_shape::DetectedObjectArray &in_objects);

  visualization_msgs::MarkerArray ObjectsToHulls(const msg_lidar_shape::DetectedObjectArray &in_objects);

  visualization_msgs::MarkerArray ObjectsToCentroids(const msg_lidar_shape::DetectedObjectArray &in_objects);

  std::string ColorToString(const std_msgs::ColorRGBA &in_color);

  void ObjVisualizationMain(const msg_lidar_shape::DetectedObjectArray &in_objects);

  bool IsObjectValid(const msg_lidar_shape::DetectedObject &in_object);

  float CheckColor(double value);

  float CheckAlpha(double value);

  std_msgs::ColorRGBA ParseColor(const std::vector<double> &in_color);

  ObjVisualization(const std::string &topic_name);
};

#endif  // CLASS_OBJ_VISUALIZATION_H
